//
//  PIDController.m
//  Podcopter
//
//  Created by Aaron Thompson on 2/23/10.
//  Copyright 2010 Vanderbilt University. All rights reserved.
//

#import "PIDController.h"

@implementation PIDController


#define outputBias 0.0
#define controllerGain 1.0
#define resetTime 1.0
#define derivativeTime 0.5

- (id)init
{
	self = [super init];
	if (self != nil) {
		lastError = 0.0;
	}
	return self;
}


- (double)updateOutputWithControllerAngle:(double)controllerAngle
							 currentAngle:(double)currentAngle
								deltaTime:(double)deltaTime
{
	double currentError = controllerAngle - currentAngle;
	double proportionalTerm = controllerGain * currentError;
	double integralTerm = controllerGain / resetTime * lastError;
	double derivativeTerm = controllerGain * derivativeTime * currentError / deltaTime;
	
	// Should this go before calculating the PID terms?
	lastError += currentError * deltaTime;
//	lastError = currentError;

	return outputBias + proportionalTerm + integralTerm + derivativeTerm;
}


@end
